

#include <opencv/cv.h>
#include <opencv/highgui.h>

#include <stdio.h>
#include <math.h>
#include <time.h>

#include <pthread.h>

#define MULTITHREAD
#define N 5
#define NUM_THREADS 50
#define MIN_JUMPS 3

struct vector {
	float v [5];
};

void printvec (struct vector v) {
	int i;

	for (i = 0; i < N; ++i) {
			  printf ("%g\t", v.v [i]);
	}

	printf ("\n");
}

struct vector zerov () {
	int i;
	struct vector v;

	for (i = 0; i < N; ++i) {
			  v.v [i] = 0.0f;
	}

	return v;
}

float norm2 (struct vector x) {
	int i;
	float res = 0.0f;
	static float w [] = {1.0, 1.0, 1.0, 0.1, 0.1};
	
	for (i = 0; i < N; ++i) {
		res += (x.v [i]) * (x.v [i]) * w [i];
		
	}

	return res;
}

struct vector divs (struct vector x, float s) {

	int i;
	
	for (i = 0; i < N; ++i) {
		x.v [i] /= s;
	}

	return x;
}

struct vector divv (struct vector n, struct vector d) {

	int i;
	
	for (i = 0; i < N; ++i) {
		n.v [i] /= d.v [i];
	}

	return n;
}

struct vector multv (struct vector n, struct vector d) {

	int i;
	
	for (i = 0; i < N; ++i) {
		n.v [i] *= d.v [i];
	}

	return n;
}


struct vector add (struct vector a, struct vector b) {

	int i;
	
	for (i = 0; i < N; ++i) {
		a.v [i] += b.v [i];
	}

	return a;
}

struct vector subv (struct vector a, struct vector b) {

	int i;
	
	for (i = 0; i < N; ++i) {
		a.v [i] -= b.v [i];
	}

	return a;
}


struct vector adds (struct vector a, float s) {
	int i;

	for (i = 0; i < N; ++i) {
		a.v [i] += s;
	};

	return a;
}

struct vector mults (struct vector x, float s) {

	int i;
	
	for (i = 0; i < N; ++i) {
		x.v [i] *= s;
	}

	return x;
}
struct vector meanshift (struct vector y, struct vector *xi, int nd, float h) {


	int i;

	struct vector x = y;
	struct vector m;
	struct vector num = zerov (), denom = zerov ();
	float temp;

	int found = 0;

	// numerator
	for (i = 0; i < nd; ++i) {

		if (sqrt (norm2 (subv (x, xi [i]))) < h) {
			num = add (num, mults (xi [i], exp (norm2 (divs (subv (x, xi [i]), h)))));
			found = 1;
		}
	}
	
	// denominator
	for (i = 0; i < nd; ++i) {
		
		if (sqrt (norm2 (subv (x, xi [i]))) < h) {
			denom = adds (denom, exp (norm2 (divs (subv (x, xi [i]), h))));
			found = 1;
		}
	}

	if (found) {
		m = subv (divv (num, denom), y);
		return m;
	}
	else {
		return num;
	}
	
}

struct vector shift (struct vector y, struct vector *xi, int nd, float h) {

	struct vector m = meanshift (y, xi, nd, h);

	//printvec (m);
	
	y = add (y, m);

	return  y;
}

struct vector trace (struct vector *xi, int nd, float h, int *count) {

	int i;
	struct vector y, yold;

	// initialize y to some random location in feature space
	for (i = 0; i < N; ++i) {
		y.v [i] = (float) rand () / (float) RAND_MAX;
	}

	*count = 0;

	do {
		yold = y;
		y = shift (y, xi, nd, h);
		++*count;
	} while (norm2 (subv (y, yold)) > 0.001f );

	return y;
}

struct vector *x = NULL;
int nd;
float h = 0.22f;
struct vector *centroids = NULL;
int ncents = 0;
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
	
void *threadstart (void *p) {
	
	int i;
	char extra = 0;
	int count;
	struct vector y;
  
	y = trace (x, nd, h, &count);

	if (count < MIN_JUMPS) {
		// not a very useful centroid
		return NULL;
	}
		  
	pthread_mutex_lock (&mutex);
		  
	// discard overlapping centroids
	for (i = 0; i < ncents; ++i) {
		if (sqrt (norm2 (subv (y, centroids [i]))) < h) {
			extra = 1;
			break;
		}
	}

	if (!extra) {
		ncents++;
		centroids = realloc (centroids, ncents * sizeof (struct vector));
		centroids [ncents - 1] = y;
	}

	pthread_mutex_unlock (&mutex);
}

int main (int argc, char *argv []) {

	int i, j, c;

	int extra;
	int label;
	float best;
	struct vector pixel;
	pthread_t threads [NUM_THREADS];
	char *file = "in.jpg";

	srand (time (NULL));

	if (argc > 1) {
		file = argv [1];
	}

	IplImage *img = cvLoadImage (file, 1);
	cvSmooth (img, img, CV_GAUSSIAN, 7, 0, 0, 0);
	IplImage *segs = cvCloneImage (img);
	IplImage *plot;
	IplImage *edges;

	if (img == NULL) {
		return -1;
	}

	CvSize sz = cvGetSize (img);
	edges = cvCreateImage (sz, img->depth, 1);

	for (i = 0; i < sz.height; ++i) {
		for (j = 0; j < sz.width; ++j) {
			
			++nd;
			x = realloc (x, nd * sizeof (struct vector));
			x [nd-1].v [3] = (float) i / (float) sz.width;
			x [nd-1].v [4] = (float) j / (float) sz.height;
			
			for (c = 0; c < 3; ++c) {
				x [nd-1].v [c] = cvGet2D (img, i, j).val [c] / 256.0f;
			}

			//printvec (x [n-1]);
		}
	}

	plot = cvCreateImage (cvSize (256, 256), IPL_DEPTH_32F, 3);

	for (i = 0; i < nd; ++i) {
		cvCircle (plot, cvPoint (x [i].v [0] * 256.0f, x [i].v [1] * 256.0f), 1, 
				CV_RGB(x [i].v [0], x [i].v [1], x [i].v [2]), 1,8,0);   
	}

	for (j = 0; j < NUM_THREADS; ++j) {
#ifdef MULTITHREAD
		pthread_create (&threads [j], NULL, threadstart, NULL);
#else
		threadstart (NULL);
#endif
	}

#ifdef MULTITHREAD
	for (j = 0; j < NUM_THREADS; ++j) {
		pthread_join (threads [j], NULL);
	}
#endif

	for (i = 0; i < sz.height; ++i) {
		for (j = 0; j < sz.width; ++j) {
			
			pixel.v [0] = cvGet2D (img, i, j).val [0] / 256.0f;
			pixel.v [1] = cvGet2D (img, i, j).val [1] / 256.0f;
			pixel.v [2] = cvGet2D (img, i, j).val [2] / 256.0f;
			pixel.v [3] = (float) i / (float) sz.width;
			pixel.v [4] = (float) j / (float) sz.height;

			label = 0;
			best = 1.0f;
			
			for (c = 0; c < ncents; ++c) {
				if (sqrt (norm2 (subv (pixel, centroids [c]))) < best) {
					label = c;
					best = sqrt (norm2 (subv (pixel, centroids [c])));
					if (best <= h) break;
				}
			}

			//float color = (float) label / (float) ncents * 256.0f;
			//cvSet2D (segs, i, j, cvScalar (color, color, color, 0));
			cvSet2D (segs, i, j, cvScalar (centroids [label].v [0] * 256, centroids [label].v [1] * 256, centroids [label].v [2] * 256, 0));
		}
	}

	cvSetImageCOI (segs, 1);
	cvCopy (segs, edges, NULL);
	cvCanny (edges, edges, 100, 300, 3);
	
	cvSaveImage ("out.jpg", plot);
	cvSaveImage ("segs.jpg", segs);
	cvSaveImage ("edges.jpg", edges);

	// pointer to pointer b/c opencv sets ptr to NULL
	cvReleaseImage (&img);
	cvReleaseImage (&plot);
	cvReleaseImage (&segs);

	return 0;
}
